
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <lib/md5.h>
#include <sys/stat.h>
#include <iostream>
#include <fstream>
#include "lib/base64.h"
#include "firmware/ver.h"
#include "firmware/firmware.h"
#include <boost/thread/thread.hpp>
#include <fcntl.h>      // read

namespace firmware {
const int SERVERPORT = 2121;

// get verison
bool Firmware::versionCallback(const std::shared_ptr<agv_srvs::srv::Version::Request> req, 
        std::shared_ptr<agv_srvs::srv::Version::Response>res)
{
    RCLCPP_INFO_STREAM(nh_.get_logger(), "ver:"<<tag<<" branch:"<<branch<<" hash:"<<hash);
    res->ver = tag;
    res->branch = branch;
    res->hash = hash;
    return true;
}


// firmware update 
bool Firmware::firmwareUpdateCallback(const std::shared_ptr<agv_srvs::srv::FirmwareUpdate::Request> req, 
        std::shared_ptr<agv_srvs::srv::FirmwareUpdate::Response> res)
{
    RCLCPP_INFO_STREAM(nh_.get_logger(), "fw name:"<<req->fw_name<<"\n fw size:"<<req->fw_size<<"\n fw md5:"<<req->fw_md5);
    std::string file_path = fw_root_dir_ + "/" + req->fw_name;
  
    // open firmware file
    std::ifstream in(file_path.c_str(), std::ifstream::binary);

    if (!in.is_open()) {
        RCLCPP_INFO_STREAM(nh_.get_logger(), "file open error.");
       
        res->success = false;
        res->message = "open error.";
        return 0;
    }

    in.seekg(0, std::ios_base::end);
    int fw_size = in.tellg();
    RCLCPP_INFO_STREAM(nh_.get_logger(), "file size:" << fw_size);
    // check file size 
    if (fw_size == 0) {
        RCLCPP_INFO_STREAM(nh_.get_logger(), "Firmware size error.");
        in.close();
        res->success = false;
        res->message = "size error.";
        return 0;
    }

    in.seekg(0, std::ios_base::beg);
    std::istreambuf_iterator<char> beg(in), end;
    std::string content;
    content.assign(beg, end);
    in.close();

    std::string m = md5(content);
    RCLCPP_INFO_STREAM(nh_.get_logger(), "MD5 Sum:" << m);
    if (m != req->fw_md5) {
        RCLCPP_INFO_STREAM(nh_.get_logger(), "Firmware md5 error.");
        // file error and delete
        if(!remove(file_path.c_str())==0) {
            RCLCPP_INFO_STREAM(nh_.get_logger(), "Faild to delete fw file.");
        } 

        res->success = false;
        res->message = "md5 error.";
        return 0;
    }
    
    // update firmware 
    std::string fifo_path = std::string("/tmp/robotguard") + std::string("/fwupdate");
    std::string cmd = "fwupdate " + req->fw_name + "\n";
    if (access(fifo_path.c_str(), F_OK) == -1)  {
        RCLCPP_INFO_STREAM(nh_.get_logger(), "Fifo file not exist.");
        res->success = false;
        res->message = fifo_path + "path error.";
        return 0;
    }
    int fifo_fd = open(fifo_path.c_str(), O_WRONLY);
    if (fifo_fd < 0) {
        RCLCPP_INFO_STREAM(nh_.get_logger(), "Fail to open fifo file.");
        res->success = false;
        res->message = "fifo error.";
    } else {
        int ret = write(fifo_fd, cmd.data(), cmd.size()); 
        if (ret <= 0) {
            RCLCPP_INFO_STREAM(nh_.get_logger(), "Fail to write fifo file.");
            res->success = false;
            res->message = "fifo write error.";
        } else {
            RCLCPP_INFO_STREAM(nh_.get_logger(), "update firmware:"<<cmd);
            res->success = true;
            res->message = "successed.";
        }
        close(fifo_fd);
    }
    
    return true;
}

// // service to ftp server start callback
// bool Firmware::ftpRequestCallback(agv_srvs::srv::FtpRequest::Request &req, agv_srvs::srv::FtpRequest::Response &res)
// {
//     if (!ftp_state_) {
//         // wait ftp retention second to stop ftp server.
//         timer_ = nh_.createTimer(rclcpp::Duration(ftp_retention_),  &Firmware::timerCallback, this, true);
//         ftp_state_ = start("lgimRobot", "Qd4XuNmKH2SxQbDX", fw_root_dir_);

//         if (ftp_state_) {
//             res.success = ftp_state_;
//             res.message = "ftp start successed.";
//         } else {
//             res.success = ftp_state_;
//             res.message = "ftp start failed.";
//         }
//     } else {
//         res.success = ftp_state_;
//         res.message = "ftp already start.";
//     }
//     return true;
// }

// void Firmware::timerCallback(const rclcpp::TimerEvent& event)
// {
//     // stop ftp server
//     stop();
//     ftp_state_ = false;
// }

void Firmware::spin()
{
    RCLCPP_INFO_STREAM(nh_.get_logger(), "start firmware thread");
}

// bool Firmware::start(const std::string usr, const std::string pwd, const std::string local_root)
// {
//     //create new ftp server instant 
//     server_ = new fineftp::FtpServer(ftp_port_);

//     //set ftp server 
//     server_->addUser(usr, pwd, local_root,  
//           fineftp::Permission::FileDelete 
//         | fineftp::Permission::FileWrite 
//         | fineftp::Permission::FileAppend);

//     //start ftp server.
//     return server_->start(1);
// }

void Firmware::init()
{
    update_ = nh_.create_service<agv_srvs::srv::FirmwareUpdate>(
        "/fw_update", 
        std::bind(&Firmware::firmwareUpdateCallback, this, std::placeholders::_1, std::placeholders::_2));

    ver_ = nh_.create_service<agv_srvs::srv::Version>(
        "/fw_version", 
        std::bind(&Firmware::versionCallback, this, std::placeholders::_1, std::placeholders::_2));
    
    // ftp_ = nh_.advertiseService("/ftp_request", &Firmware::ftpRequestCallback, this);
    //get params
    fw_root_dir_ = nh_.declare_parameter<std::string>("firmware_dir", std::string(getenv("HOME")));
    RCLCPP_INFO_STREAM(nh_.get_logger(), "firmware_dir: "<<fw_root_dir_);
    ftp_root_dir_ = nh_.declare_parameter<std::string>("ftp_dir", std::string(getenv("HOME")));
    RCLCPP_INFO_STREAM(nh_.get_logger(), "ftp_dir: "<<ftp_root_dir_);
    ftp_port_ = nh_.declare_parameter<int>("ftp_port", SERVERPORT);
    RCLCPP_INFO_STREAM(nh_.get_logger(), "ftp_port: "<<ftp_port_);
    ftp_retention_ = nh_.declare_parameter<int>("ftp_retention", 60);
    RCLCPP_INFO_STREAM(nh_.get_logger(), "ftp_retention: "<<ftp_retention_);
}

}  // end namespace

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("firmware");
    setlocale(LC_ALL, "");

    firmware::Firmware fw(*node);
    fw.init();
    
    // boost::thread t = boost::thread(boost::bind(&firmware::Firmware::spin, &fw));
    rclcpp::spin(node);
    // t.interrupt();
    // t.join();
    
    return 0;
}
